''*******************************************
''*           Playing with IMU data          *
''********************************************

{-----------------REVISION HISTORY-----------------
 v0.1 - 4/17/2010 - Initial testing
}

CON
  _clkmode = xtal1 + pll16x
  _xinfreq = 5_000_000
  IMU_Baud = 115_200
  IMU_rx   = 11
  IMU_tx   = 10

VAR
  byte Rx, Tx
  byte buffer[36]
  long cptr
  long x,y,z,pitch,roll,yaw
  word RunningCount,ax,ay,az,gx,gy,gz

OBJ
  IMU   : "FullDuplexSerialPlus"
  debug : "FullDuplexSerialPlus"

PUB Main | bytebuff,pointer
  IMU.start(IMU_tx, IMU_rx, 0, IMU_Baud)    'Setup serial connections
  debug.start(31, 30, 0, 115_200)

  waitcnt(clkfreq+cnt)

  IMU.str(string("%"))                      'Set IMU sensitivity
  IMU.str(string("*"))                      'Set IMU sample freq
  IMU.str(string("#"))                      'Start IMU

  repeat
    readIMU
    debug.dec(RunningCount)
    debug.str(STRING(","))
    debug.dec(ax)
    debug.str(STRING(","))
    debug.dec(ay)
    debug.str(STRING(","))
    debug.dec(az)
    debug.str(STRING(","))
    debug.dec(gx)
    debug.str(STRING(","))
    debug.dec(gy)
    debug.str(STRING(","))
    debug.dec(gz)
    debug.str(STRING(10))

PUB readIMU
  Rx:=0                                 'initialize, so we know definitely what's in before checking content in the while
  repeat while Rx <> "A"                ' wait for the "A" to insure we are starting with
    Rx := IMU.rxcheck

  cptr := 0                               ' zero the buffer pointer to start
  byte[cptr++] := Rx                    ' NOW the A is really in the buffer

  repeat while Rx <> "Z" and cptr < 16  ' continue to collect data until the end of sentence ("Z")
    Rx := IMU.rx                        ' get character from uart Rx
    buffer[cptr++] := Rx              ' save the character in the buff

  RunningCount := buffer[ 1]<<8 + buffer[ 2]  'Put buffer data into variables
  ax := buffer[ 3]<<8 + buffer[ 4]
  ay := buffer[ 5]<<8 + buffer[ 6]
  az := buffer[ 7]<<8 + buffer[ 8]
  gx := buffer[ 9]<<8 + buffer[ 10]
  gy := buffer[ 11]<<8 + buffer[ 12]
  gz := buffer[ 13]<<8 + buffer[ 14]

return                                  ' return to calling routine

